'''
    Physics helper module, uses ODE
   
  
    Extends basic Model to provide
        - DynamicModel - has body (is dynamic, i.e. obeys physical rules)
        - RigidModel - has body and geom (collides and is dynamic)
        - StaticModel - has geom (collides but is static)
        
    Also provides wraps for Mass and Geom
    
    Each Model type also includes a set of basic primitives
    
'''
import ode
import fudge
from fudge.core import *
from fudge.models import Model, Cube, Sphere, Empty, EmptyMesh
from fudge.core import singleton
from fudge.types import Matrix3, Matrix4, Vector3



SPF = 0.02       # inverse FPS, get it?
world = ode.World()
space = ode.Space()
contactgroup = ode.JointGroup()
geoms = []
joints = []


@singleton
class Physics:
    ''' Handles physical simulation '''
    def __init__(self):
        world.setGravity((0.0, -5.0, 0.0))
        world.setERP(0.2)
        world.setCFM(10E-5)
        self.dt = 0
        self.paused = True
        self.friction = 0
    
    def Initialize(self):
        self.Play()
            
    def setFriction(self, val):
        self.friction = val
        
    def __Collision_callback(self, args, geom1, geom2):
        """Callback function for the collide() method.

        This function checks if the given geoms do collide and
        creates contact joints if they do.
        """

        # Check if the objects do collide
        contacts = ode.collide(geom1, geom2)

        # Create contact joints
        world, contactgroup = args
        for c in contacts:
            c.setBounce(0.1)            # TODO: customizable
            c.setMu(self.friction)               # TODO: customizable
            j = ode.ContactJoint(world, contactgroup, c)
            j.attach(geom1.getBody(), geom2.getBody())
    
    def Pause(self):
        ''' Pauses physics '''
        self.Do = self.__PausePhysics
        self.paused = True
    
    def Play(self):
        ''' Activates physics '''
        self.Do = self.__PlayPhysics
        self.paused = False
       
    def HACK(self, fn):
        self.__fn = fn
    
    def __PlayPhysics(self, dt):
        ''' Steps physics (needs to be quantized at constant intervals) '''
        self.dt += dt
        while not self.dt < SPF:
            self.__fn() # HACK!!!!
            space.collide((world, contactgroup), self.__Collision_callback)
            world.step(SPF)
            contactgroup.empty()
            self.dt -= SPF
            
    def __PausePhysics(self, dt):
        pass
        
    def Do(self, dt):
        ''' set to Play or Pause Physics '''
        pass

class DynamicModel(Model):
    '''
        Has one physical body, which is controlled by physics. Updates position and rotation prior to display.
        
        Parameter is an ODE Mass object. You can use helpers to create it.
        
        Note that the draw routine is overriden in order to draw in World coordinates
    '''
    
    
    def __init__(self, mass, **kwargs):
        Model.__init__(self, **kwargs)
        self.body = ode.Body(world)
        self.body.setMass(mass._M)
        self.body.setPosition(self._Position)
        self.body.setRotation(self._Transform.getUpper().flatten())
        
    def _transform(self):
        # set my new position
        self._Position.setTo(*self.body.getPosition())
        self._Transform.embed(Matrix3(self.body.getRotation(), transpose = True))    # uff....
        Model._transform(self)
        
        
class StaticModel(Model):
    '''
        Provides collision geometry but in itself is otherwise unaffected by physics
        
        parameter is a list of geoms. Use helpers to create it
    '''
    def __init__(self, geom, mesh = EmptyMesh(), **kwargs):
        super(self.__class__, self).__init__(mesh, **kwargs)
        self.geom = geom
        try:
            self.geom._G.setPosition(self._Position)
        except Exception:
            pass
        # TODO: set rotation?

    @staticmethod
    def Floor(height, **kwargs):
        return StaticModel(geom = Geom.Floor(height), mesh = EmptyMesh(), **kwargs)
        
class RigidModel(DynamicModel):
    '''
        Dynamic Model with a collision geom
    '''
    def __init__(self, mass, geom, **kwargs):
        super(self.__class__, self).__init__(mass, **kwargs)
        self.geom = geom
        self.geom._G.setBody(self.body)
        self.geom._G.setPosition(self._Position)
        self.geom._G.setRotation(self._Transform.getUpper().flatten())
        
    @staticmethod
    def Cuboid(sx, sy, sz, density = 1.0, **kwargs):
        return RigidModel(mesh = Cube.Mesh(1.0), scale = Vector3(sx, sy, sz), mass = Mass.Cuboid(sx, sy, sz, density), geom = Geom.Cuboid(sx * 1.01, sy * 1.01, sz * 1.01),**kwargs)

    @staticmethod
    def Sphere(radius, density = 1.0, **kwargs):
        return RigidModel(mesh = Sphere.Mesh(radius), mass = Mass.Sphere(radius, density), geom = Geom.Sphere(radius * 1.01), **kwargs)
        
        
@singleton
class EnvironmentModel(Model):
    def __init__(self):
        self.body = ode.environment
        
class Mass(object):
    def __init__(self, mass):
        '''
            creates fudge Mass object from ODE mass
        '''
        self._M = mass
    
    @staticmethod
    def Sphere(size, density):
        i = Mass(ode.Mass())
        i._M.setSphere(density, size)
        return i
        
    @staticmethod
    def Cuboid(sx, sy, sz, density = 1.0):
        i = Mass(ode.Mass())
        i._M.setBox(density, sx, sy, sz)
        return i
        
class Geom(object):
    ''' Wrapper for ODE geom, used for collision '''
    def __init__(self, geom):
        self._G = geom

    @staticmethod
    def Floor(height):
        ''' Horizonal (surface-level) infinite Plane '''
        return Geom(ode.GeomPlane(space, (0,1,0), height))
    
    @staticmethod
    def Plane(normal, dist):
        return Geom(ode.GeomPlane(space, normal, dist))
    
    @staticmethod
    def Cuboid(sx, sy, sz):
        return Geom(ode.GeomBox(space, lengths = (sx,sy,sz)))

    @staticmethod
    def Cube(size):
        return Geom(ode.GeomBox(space, lengths = (size, size, size)))
        
    @staticmethod
    def Sphere(radius):
        return Geom(ode.GeomSphere(space, radius))
    
    @staticmethod
    def Capsule(radius, length):
        return Geom(ode.GeomCapsule(space, radius, length))
        
class Joint(object):
    ''' Wrapper for joints. Uses Rigid/Dynamic models '''
    def __init__(self, joint):
        self._J = joint
        joints.append(joint)
    
    @staticmethod
    def Ball(px, py, pz, model1, model2 = EnvironmentModel()):
        j = Joint(ode.BallJoint(world))
        j._J.attach(model1.body, model2.body)
        j._J.setAnchor((px,py,pz))
        return j
        
#
# Pre-defines and Built-ins
#

def RagDoll(z = 0.0):
    m = Empty()
    t = m.addModel(RigidModel.Cuboid(1.0, 2.0, 0.25, position = Vector3(0., 3.0, z))) # torso
    llu = m.addModel(RigidModel.Cuboid(0.25, 0.8, 0.25, position = Vector3(-0.4, 1.4, z))) # legs upper
    lru = m.addModel(RigidModel.Cuboid(0.25, 0.8, 0.25, position = Vector3(0.4, 1.4, z)))
    lll = m.addModel(RigidModel.Cuboid(0.25, 0.8, 0.25, position = Vector3(-0.4, 0.5, z))) # legs lower
    lrl = m.addModel(RigidModel.Cuboid(0.25, 0.8, 0.25, position = Vector3(0.4, 0.5, z)))
    h = m.addModel(RigidModel.Sphere(0.45, position = Vector3(0., 4.5, z)))
    alu = m.addModel(RigidModel.Cuboid(0.25, 1.05, 0.25, position = Vector3(-0.65, 3.4, z))) # arms upper
    aru = m.addModel(RigidModel.Cuboid(0.25, 1.05, 0.25, position = Vector3(0.65, 3.4, z)))
    all = m.addModel(RigidModel.Cuboid(0.25, 1.05, 0.25, position = Vector3(-0.65, 2.3, z))) # arms lower
    arl = m.addModel(RigidModel.Cuboid(0.25, 1.05, 0.25, position = Vector3(0.65, 2.3, z)))

    Joint.Ball(0.4, 1.0, z, lru, lrl)  # join legs
    Joint.Ball(-0.4, 1.0, z, llu, lll)
    Joint.Ball(-0.4, 1.9, z, llu, t)   # to torso
    Joint.Ball(0.4, 1.9, z, lru, t)
    Joint.Ball(0.0, 4.0, z, h, t)  # head
    Joint.Ball(0.6, 2.85, z, aru, arl)  # join arms
    Joint.Ball(-0.6, 2.85, z, alu, all)
    Joint.Ball(-0.55, 4.1, z, alu, t)
    Joint.Ball(0.55, 4.1, z, aru, t)
    
    return m